function [state, P] = ekf_update(state, P, z, landmark_idx, R)
% EKF更新步骤
% 输入: state-状态向量, P-协方差, z-观测[r;φ], landmark_idx-路标索引, R-观测噪声
% 输出: state-更新状态, P-更新协方差

    x = state(1);
    y = state(2);
    theta = state(3);
    
    m_idx = 3 + (landmark_idx - 1) * 2;
    mx = state(m_idx + 1);
    my = state(m_idx + 2);
    
    % 预测观测
    dx = mx - x;
    dy = my - y;
    q = dx^2 + dy^2;
    range_pred = sqrt(q);
    bearing_pred = wrapToPi(atan2(dy, dx) - theta);
    z_pred = [range_pred; bearing_pred];
    
    % 观测雅可比
    n = length(state);
    H = zeros(2, n);
    H(1, 1) = -dx / range_pred;
    H(1, 2) = -dy / range_pred;
    H(1, 3) = 0;
    H(1, m_idx + 1) = dx / range_pred;
    H(1, m_idx + 2) = dy / range_pred;
    H(2, 1) = dy / q;
    H(2, 2) = -dx / q;
    H(2, 3) = -1;
    H(2, m_idx + 1) = -dy / q;
    H(2, m_idx + 2) = dx / q;
    
    % 创新
    innovation = z - z_pred;
    innovation(2) = wrapToPi(innovation(2));
    
    % 卡尔曼增益
    S = H * P * H' + R;
    K = P * H' / S;
    
    % 状态更新
    state = state + K * innovation;
    state(3) = wrapToPi(state(3));
    
    % Joseph形式协方差更新
    I_KH = eye(n) - K * H;
    P = I_KH * P * I_KH' + K * R * K';
    P = (P + P') / 2;
    P = P + eye(n) * 1e-9;
end

